/*
 * OREKIT-X
 * Copyright 2002-2008 CS Communication & Systemes
 * 
 * Parts of this software package have been licensed to CS
 * Communication & Systemes (CS) under one or more contributor license
 * agreements.  See the NOTICE file distributed with this work for
 * additional information.
 *  
 * This is an experimental copy of OREKIT from www.orekit.org.
 * Please use the original OREKIT from orekit.org for normal work
 * unrelated to this research project.
 * 
 * Licensed under the Apache License, Version 2.0 (the "License"); you
 * may not use this file except in compliance with the License.  You
 * may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
 * implied.  See the License for the specific language governing
 * permissions and limitations under the License.
 */
package ore;

import org.apache.commons.math.geometry.Rotation;
import org.apache.commons.math.geometry.Vector3D;


/**
 * This class represents the transformation engine between {@link
 * Frame frames}.  It is used both to define the relationship between
 * each frame and its parent frame and to gather all individual
 * transforms into one operation when converting between frames far
 * away from each other.
 * 
 * A transformation in three dimensional space can be uniquely
 * represented as an elementary translation followed by an elementary
 * rotation.
 * 
 * The convention used in OREKIT is vectorial transformation. It means
 * that a transformation is defined as a transform to apply to the
 * coordinates of a vector expressed in the old frame to obtain the
 * same vector expressed in the new frame.
 *
 * Instances of this class are immutable.
 *
 * <h5> Example </h5>
 * 
 * Example of translation from R<sub>A</sub> to R<sub>B</sub>.
 * Transform the {@link PositionVelocity} PV<sub>A</sub> to
 * PV<sub>B</sub>.
 * 
 * <pre>
 * PV<sub>A</sub> = ({1, 0, 0} , {1, 0, 0});
 * PV<sub>B</sub> = ({0, 0, 0} , {0, 0, 0});
 *
 * Vector3D translation = new Vector3D(-1,0,0);
 * Vector3D velocity = new Vector3D(-1,0,0);
 *
 * Transform R1toR2 = new Transform(translation, Velocity);
 *
 * PV<sub>B</sub> = R1toR2.transformPositionVelocity(PV<sub>A</sub>);
 * </pre>
 *
 * Example of rotation from R<sub>A</sub> to R<sub>B</sub>.  Transform
 * the {@link PositionVelocity} PV<sub>A</sub> to PV<sub>B</sub>.
 *
 * <pre>
 * PV<sub>A</sub> = ({1, 0, 0}, {1, 0, 0});
 * PV<sub>B</sub> = ({0, 1, 0}, {-2, 1, 0});
 *
 * Rotation rotation = new Rotation(Vector3D.PLUS_K, Math.PI / 2);
 * Vector3D rotationRate = new Vector3D(0, 0, -2);
 *
 * Transform R1toR2 = new Transform(rotation, rotationRate);
 *
 * PV<sub>B</sub> = R1toR2.transformPositionVelocity(PV<sub>A</sub>);
 * </pre>
 *
 * @author Luc Maisonobe
 * @author Fabien Maussion
 */
public class Transform 
    extends Object
    implements java.io.Serializable
{
    public static final Transform IDENTITY =
        new Transform(Vector3D.ZERO, Vector3D.ZERO, Rotation.IDENTITY, Vector3D.ZERO);



    public final Vector3D translation;
    public final Vector3D velocity;
    public final Rotation rotation;
    /**
     * First derivative of the rotation wrt time (norm representing
     * angular rate).
     */
    public final Vector3D rotationRate;


    /** 
     * @param translation first primitive operation to apply
     * @param velocity first time derivative of the translation
     * @param rotation second primitive operation to apply
     * @param rotationRate first time derivative of the rotation (norm representing angular rate)
     */
    protected Transform(Vector3D translation, Vector3D velocity,
                        Rotation rotation, Vector3D rotationRate)
    {
        super();
        this.translation  = translation;
        this.rotation     = rotation;
        this.velocity     = velocity;
        this.rotationRate = rotationRate;
    }
    /** 
     * @param translation translation to apply (i.e. coordinates of
     * the transformed origin, or coordinates of the origin of the
     * old frame in the new frame)
     */
    public Transform(Vector3D translation) {
        this(translation, Vector3D.ZERO, Rotation.IDENTITY, Vector3D.ZERO);
    }
    /** 
     * @param rotation rotation to apply ( i.e. rotation to apply to the
     * coordinates of a vector expressed in the old frame to obtain the
     * same vector expressed in the new frame )
     */
    public Transform(Rotation rotation) {
        this(Vector3D.ZERO, Vector3D.ZERO, rotation, Vector3D.ZERO);
    }
    /** 
     * @param translation translation to apply (i.e. coordinates of
     * the transformed origin, or coordinates of the origin of the
     * old frame in the new frame)
     * @param velocity the velocity of the translation (i.e. origin
     * of the old frame velocity in the new frame)
     */
    public Transform(Vector3D translation, Vector3D velocity) {
        this(translation, velocity, Rotation.IDENTITY, Vector3D.ZERO);
    }
    /** 
     * @param rotation rotation to apply ( i.e. rotation to apply to the
     * coordinates of a vector expressed in the old frame to obtain the
     * same vector expressed in the new frame )
     * @param rotationRate the axis of the instant rotation
     * expressed in the new frame. (norm representing angular rate)
     */
    public Transform(Rotation rotation, Vector3D rotationRate) {
        this(Vector3D.ZERO, Vector3D.ZERO, rotation, rotationRate);
    }
    /** 
     * @param first first transform applied
     * @param second second transform applied
     */
    public Transform(Transform first, Transform second) {
        this(CompositeTranslation(first, second), CompositeVelocity(first, second),
             CompositeRotation(first, second), CompositeRotationRate(first, second));
    }


    /** 
     * @return Underlying elementary translation
     * @see #getRotation()
     */
    public Vector3D getTranslation() {
        return translation;
    }
    /** 
     * @return First time derivative of the translation
     */
    public Vector3D getVelocity() {
        return velocity;
    }
    /** 
     * @return Underlying elementary rotation
     * @see #getTranslation()
     */
    public Rotation getRotation() {
        return rotation;
    }
    /** 
     * The norm represents the angular rate.
     * @return First time derivative of the rotation
     */
    public Vector3D getRotationRate() {
        return rotationRate;
    }
    /** 
     * @return inverse transform of the instance
     */
    public Transform getInverse() {
        Vector3D rT = rotation.applyTo(translation);
        return new Transform(rT.negate(),
                             Vector3D.crossProduct(rotationRate, rT).subtract(rotation.applyTo(velocity)),
                             rotation.revert(),
                             rotation.applyInverseTo(rotationRate.negate()));
    }
    /** 
     * Rotate and translate position.
     * 
     * @param position Position to transform
     * @return transformed position
     */
    public Vector3D transformPosition(Vector3D position) {
        return rotation.applyTo(translation.add(position));
    }
    /** 
     * Rotate direction.
     * 
     * @param direction Vector to transform
     * @return transformed vector
     */
    public Vector3D transformVector(Vector3D direction) {
        return rotation.applyTo(direction);
    }
    /** 
     * @param line to transform
     * @return transformed line
     */
    public Line transformLine(Line line) {
        Vector3D transformedOrigin    = transformPosition(line.getOrigin());
        Vector3D transformedDirection = transformVector(line.getDirection());
        return new Line(transformedOrigin, transformedDirection);
    }
    /** 
     * @param pv the couple position-velocity to transform.
     * @return transformed position/velocity
     */
    public PositionVelocity transformPositionVelocity(PositionVelocity pv) {
        Vector3D p = pv.getPosition();
        Vector3D v = pv.getVelocity();
        Vector3D transformedP = rotation.applyTo(translation.add(p));
        Vector3D cross = Vector3D.crossProduct(rotationRate, transformedP);
        return new PositionVelocity(transformedP,
                                 rotation.applyTo(v.add(velocity)).subtract(cross));
    }



    protected static Vector3D CompositeTranslation(Transform first, Transform second) {
        return first.translation.add(first.rotation.applyInverseTo(second.translation));
    }
    protected static Vector3D CompositeVelocity(Transform first, Transform second) {
        Vector3D cross =
            Vector3D.crossProduct(first.rotationRate, second.translation);
        return first.velocity.add(first.rotation.applyInverseTo(second.velocity.add(cross)));
    }
    protected static Rotation CompositeRotation(Transform first, Transform second) {
        return second.rotation.applyTo(first.rotation);
    }
    protected static Vector3D CompositeRotationRate(Transform first, Transform second) {
        return second.rotationRate.add(second.rotation.applyTo(first.rotationRate));
    }
}
